import numpy as np
import math
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from mpl_toolkits.mplot3d import Axes3D
plt.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei', 'DejaVu Sans']  # 黑体、微软雅黑
plt.rcParams['axes.unicode_minus'] = False  # 解决负号显示问题
# ====== 常量与场景 ======
g = 9.81  # m/s^2
R = 10.0  # 有效遮蔽半径
v_sink = 3  # 起爆后云团中心下沉速度 (m/s)
T = np.array([0, 200, 0])  # 真目标中心
Of = np.array([0, 0, 0])  # 假目标（原点）

# 导弹 M1
M0 = np.array([20000, 0, 2000])
uM = (Of - M0) / np.linalg.norm(Of - M0)
vM = 300 * uM  # 300 m/s 指向假目标
L = np.linalg.norm(Of - M0)
t_hit = L / 300  # 导弹到达假目标时刻≈ 67 s

# FY1（等高度飞行，z=1800 m）
F0 = np.array([17800, 0, 1800])
v_min = 70
v_max = 140

# ====== 搜索设置（可按需调节速度/精度）======
# 航向角与速度的粗网格
angles = np.deg2rad(np.arange(-180, 180, 15))  # 相对 +x 轴，逆时针为正
speeds = np.arange(70, 141, 10)  # m/s

# 三枚弹的起爆时刻扫描（全局时刻）；引信延时候选
dt_eval = 0.02  # 时间评估步长（全局）
t_grid = np.arange(2, t_hit - 2, 2)  # 候选起爆时刻（2 s 步长）
tau_set = np.array([3.0, 3.6, 4.2])  # 候选引信延时（s）
min_release_gap = 1.0  # 相邻两次"投放"时间间隔≥ 1 s
tau_min = 0.5  # 最小引信延时（安全下限）
tau_max = 6.0  # 最大引信延时（便于搜索）

# ====== 预计算导弹轨迹与"导弹→真目标"视线量 ======
ts = np.arange(0, t_hit + dt_eval, dt_eval)  # 全局时间轴
N = len(ts)
Mx = M0[0] + vM[0] * ts
My = M0[1] + vM[1] * ts
Mz = M0[2] + vM[2] * ts

Ux = T[0] - Mx
Uy = T[1] - My
Uz = T[2] - Mz  # 视线向量
UU = Ux ** 2 + Uy ** 2 + Uz ** 2

# ====== 贪心三弹搜索（对每个θ、v 逐弹选最优）======
best_total = -np.inf
best_plan = {}

for ia, theta in enumerate(angles):
    uxF = np.cos(theta)
    uyF = np.sin(theta)

    for iv, vF in enumerate(speeds):
        # 逐弹贪心：初始化（当前联合最小距离为 +Inf）
        dmin_all = np.full(N, np.inf)
        shots = []
        last_t_rel = -np.inf  # 上一次"投放"时间（约束间隔）

        for k in range(3):  # 三枚弹
            best_k_gain = -np.inf
            best_k = {}

            for t_det in t_grid:
                for tau in tau_set:
                    if tau < tau_min or tau > tau_max:
                        continue

                    t_rel = t_det - tau
                    if t_rel < 0:
                        continue

                    if t_rel < last_t_rel + min_release_gap:
                        continue

                    # 起爆点（水平由 FY1 位移给出，竖直由自由落体给出）
                    x_det = F0[0] + vF * uxF * t_det
                    y_det = F0[1] + vF * uyF * t_det
                    z_det = 1800 - 0.5 * g * tau ** 2

                    # 云团中心轨迹（仅在 [t_det, t_det+20] 内有效）
                    Cz = z_det - v_sink * (ts - t_det)  # 数组
                    valid = (ts >= t_det) & (ts <= t_det + 20)

                    # 计算该弹对"导弹→真目标"线段的最小距离 d_k(t)
                    Wx = x_det - Mx
                    Wy = y_det - My
                    Wz = Cz - Mz

                    WU = Wx * Ux + Wy * Uy + Wz * Uz
                    lam = WU / UU
                    lam = np.clip(lam, 0, 1)

                    Px = Mx + lam * Ux
                    Py = My + lam * Uy
                    Pz = Mz + lam * Uz

                    dx = x_det - Px
                    dy = y_det - Py
                    dz = Cz - Pz

                    d_k = np.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
                    d_k[~valid] = np.inf  # 窗口外无效

                    # 与现有联合取最小
                    dmin_new = np.minimum(dmin_all, d_k)
                    mask_new = (dmin_new <= R)
                    gain = np.sum(mask_new) * dt_eval  # 联合遮蔽总时长（秒）

                    if gain > best_k_gain:
                        best_k_gain = gain
                        best_k = {
                            't_det': t_det,
                            'tau': tau,
                            't_rel': t_rel,
                            'x_det': x_det,
                            'y_det': y_det,
                            'z_det': z_det,
                            'dmin_new': dmin_new.copy()
                        }

            # 若该轮未找到可行解，则提前结束
            if not best_k:
                break

            # 接受该弹的最优选择，更新联合 dmin_all
            dmin_all = best_k['dmin_new']
            last_t_rel = best_k['t_rel']

            # 记录该弹信息
            shot_info = {
                't_det': best_k['t_det'],
                'tau': best_k['tau'],
                't_rel': best_k['t_rel'],
                'D': np.array([F0[0] + vF * uxF * best_k['t_rel'],
                               F0[1] + vF * uyF * best_k['t_rel'],
                               1800]),
                'C0': np.array([best_k['x_det'], best_k['y_det'], best_k['z_det']])
            }
            shots.append(shot_info)

        total_cov = np.sum(dmin_all <= R) * dt_eval
        if total_cov > best_total:
            best_total = total_cov
            best_plan = {
                'theta': theta,
                'vF': vF,
                'shots': shots,
                'dmin': dmin_all.copy()
            }

# ====== 输出最优策略 ======
print("========= 问题 3 最优三弹遮蔽策略（FY1→M1） =========")
print(f"航向角θ = {np.rad2deg(best_plan['theta']):.2f}°（相对 +x 逆时针）")
print(f"飞行速度 v = {best_plan['vF']:.2f} m/s")

for k, s in enumerate(best_plan['shots'], 1):
    print(f"——第{k} 枚——")
    print(f"投放时刻 t_rel = {s['t_rel']:.3f} s （与上一枚间隔满足≥1 s）")
    print(f"引信延时τ = {s['tau']:.3f} s")
    print(f"起爆时刻 t_det = {s['t_det']:.3f} s")
    print(f"投放点 D{k} = ({s['D'][0]:.3f}, {s['D'][1]:.3f}, {s['D'][2]:.3f}) m")
    print(f"起爆点 C{k} = ({s['C0'][0]:.3f}, {s['C0'][1]:.3f}, {s['C0'][2]:.3f}) m")

print(f"三弹联合最大遮蔽总时长 = {best_total:.6f} s")

# 理论上单弹≤20 s，三弹≤60 s（若超过 60 s，说明设置/步长需检查）
if best_total > 60 + 1e-6:
    print("警告：遮蔽总时长超过 60 s，建议检查搜索/评估设置。")

# ====== 可视化 ======
# 提取最优策略参数
theta = best_plan['theta']
vF = best_plan['vF']
shots = best_plan['shots']
dmin_all = best_plan['dmin']

# 计算FY1飞行轨迹
uxF = np.cos(theta)
uyF = np.sin(theta)
t_max = max([s['t_rel'] for s in shots]) + 5  # 额外显示5秒
t_fy = np.linspace(0, t_max, 100)
x_fy = F0[0] + vF * uxF * t_fy
y_fy = F0[1] + vF * uyF * t_fy
z_fy = np.full_like(t_fy, 1800)

# 计算导弹轨迹
t_missile = np.linspace(0, t_hit, 100)
x_missile = M0[0] + vM[0] * t_missile
y_missile = M0[1] + vM[1] * t_missile
z_missile = M0[2] + vM[2] * t_missile

# 计算视线线段上的点（导弹到目标）
t_line = np.linspace(0, 1, 100)
x_line = M0[0] + (T[0] - M0[0]) * t_line
y_line = M0[1] + (T[1] - M0[1]) * t_line
z_line = M0[2] + (T[2] - M0[2]) * t_line

# 创建3D图形
fig = plt.figure(figsize=(15, 10))

# 3D轨迹图
ax1 = fig.add_subplot(2, 2, 1, projection='3d')
ax1.plot(x_fy, y_fy, z_fy, 'b-', label='FY1飞行路径')
ax1.plot(x_missile, y_missile, z_missile, 'r-', label='导弹轨迹')
ax1.plot(x_line, y_line, z_line, 'g--', label='导弹-目标视线')

# 标记关键点
ax1.scatter(*F0, color='blue', marker='o', s=50, label='FY1起始点')
ax1.scatter(*M0, color='red', marker='o', s=50, label='导弹起始点')
ax1.scatter(*T, color='green', marker='*', s=100, label='真目标')
ax1.scatter(*Of, color='orange', marker='x', s=100, label='假目标')

# 标记投放点和起爆点
colors = ['purple', 'cyan', 'magenta']
for i, s in enumerate(shots):
    ax1.scatter(*s['D'], color=colors[i], marker='^', s=80, label=f'投放点{i + 1}')
    ax1.scatter(*s['C0'], color=colors[i], marker='s', s=80, label=f'起爆点{i + 1}')

ax1.set_xlabel('X (m)')
ax1.set_ylabel('Y (m)')
ax1.set_zlabel('Z (m)')
ax1.set_title('3D轨迹图')
ax1.legend(loc='upper left', bbox_to_anchor=(0, 1))

# 遮蔽效果时间序列
ax2 = fig.add_subplot(2, 2, 2)
ax2.plot(ts, dmin_all, 'b-', label='最小距离')
ax2.axhline(y=R, color='r', linestyle='--', label='有效遮蔽半径')
ax2.set_xlabel('时间 (s)')
ax2.set_ylabel('最小距离 (m)')
ax2.set_title('遮蔽效果时间序列')
ax2.legend()
ax2.grid(True)

# 投放时间点
for i, s in enumerate(shots):
    ax2.axvline(x=s['t_rel'], color=colors[i], linestyle=':', label=f'投放{i + 1}')
    ax2.axvline(x=s['t_det'], color=colors[i], linestyle='--', label=f'起爆{i + 1}')

# XY平面投影
ax3 = fig.add_subplot(2, 2, 3)
ax3.plot(x_fy, y_fy, 'b-', label='FY1飞行路径')
ax3.plot(x_missile, y_missile, 'r-', label='导弹轨迹')
ax3.plot(x_line, y_line, 'g--', label='导弹-目标视线')

# 标记关键点
ax3.scatter(*F0[:2], color='blue', marker='o', s=50, label='FY1起始点')
ax3.scatter(*M0[:2], color='red', marker='o', s=50, label='导弹起始点')
ax3.scatter(*T[:2], color='green', marker='*', s=100, label='真目标')
ax3.scatter(*Of[:2], color='orange', marker='x', s=100, label='假目标')

# 标记投放点和起爆点
for i, s in enumerate(shots):
    ax3.scatter(s['D'][0], s['D'][1], color=colors[i], marker='^', s=80, label=f'投放点{i + 1}')
    ax3.scatter(s['C0'][0], s['C0'][1], color=colors[i], marker='s', s=80, label=f'起爆点{i + 1}')

ax3.set_xlabel('X (m)')
ax3.set_ylabel('Y (m)')
ax3.set_title('XY平面投影')
ax3.legend()
ax3.grid(True)

# XZ平面投影
ax4 = fig.add_subplot(2, 2, 4)
ax4.plot(x_fy, z_fy, 'b-', label='FY1飞行路径')
ax4.plot(x_missile, z_missile, 'r-', label='导弹轨迹')
ax4.plot(x_line, z_line, 'g--', label='导弹-目标视线')

# 标记关键点
ax4.scatter(F0[0], F0[2], color='blue', marker='o', s=50, label='FY1起始点')
ax4.scatter(M0[0], M0[2], color='red', marker='o', s=50, label='导弹起始点')
ax4.scatter(T[0], T[2], color='green', marker='*', s=100, label='真目标')
ax4.scatter(Of[0], Of[2], color='orange', marker='x', s=100, label='假目标')

# 标记投放点和起爆点
for i, s in enumerate(shots):
    ax4.scatter(s['D'][0], s['D'][2], color=colors[i], marker='^', s=80, label=f'投放点{i + 1}')
    ax4.scatter(s['C0'][0], s['C0'][2], color=colors[i], marker='s', s=80, label=f'起爆点{i + 1}')

ax4.set_xlabel('X (m)')
ax4.set_ylabel('Z (m)')
ax4.set_title('XZ平面投影')
ax4.legend()
ax4.grid(True)

plt.tight_layout()
plt.show()

# 创建遮蔽效果动画
fig_anim = plt.figure(figsize=(10, 8))
ax_anim = fig_anim.add_subplot(111, projection='3d')

# 绘制静态元素
ax_anim.plot(x_fy, y_fy, z_fy, 'b-', alpha=0.3, label='FY1飞行路径')
ax_anim.plot(x_missile, y_missile, z_missile, 'r-', alpha=0.3, label='导弹轨迹')
ax_anim.plot(x_line, y_line, z_line, 'g--', alpha=0.3, label='导弹-目标视线')

# 标记关键点
ax_anim.scatter(*F0, color='blue', marker='o', s=50, label='FY1起始点')
ax_anim.scatter(*M0, color='red', marker='o', s=50, label='导弹起始点')
ax_anim.scatter(*T, color='green', marker='*', s=100, label='真目标')
ax_anim.scatter(*Of, color='orange', marker='x', s=100, label='假目标')

# 标记投放点和起爆点
for i, s in enumerate(shots):
    ax_anim.scatter(*s['D'], color=colors[i], marker='^', s=80, label=f'投放点{i + 1}')
    ax_anim.scatter(*s['C0'], color=colors[i], marker='s', s=80, label=f'起爆点{i + 1}')

# 初始化云团
clouds = []
for i in range(3):
    cloud = ax_anim.scatter([], [], [], color=colors[i], alpha=0.5, s=100)
    clouds.append(cloud)

ax_anim.set_xlabel('X (m)')
ax_anim.set_ylabel('Y (m)')
ax_anim.set_zlabel('Z (m)')
ax_anim.set_title('遮蔽效果动态演示')
ax_anim.legend(loc='upper left', bbox_to_anchor=(0, 1))


# 动画更新函数
def update(frame):
    t_current = frame * 0.5  # 时间进度（每帧0.5秒）

    # 更新导弹位置
    missile_pos = M0 + vM * t_current
    ax_anim.title.set_text(f'遮蔽效果动态演示 - 时间: {t_current:.1f}s')

    # 更新云团位置
    for i, s in enumerate(shots):
        if t_current >= s['t_det'] and t_current <= s['t_det'] + 20:
            # 云团存在期间
            cloud_center = s['C0'] - np.array([0, 0, v_sink * (t_current - s['t_det'])])
            clouds[i]._offsets3d = ([cloud_center[0]], [cloud_center[1]], [cloud_center[2]])
        else:
            # 云团不存在
            clouds[i]._offsets3d = ([], [], [])

    return clouds


# 创建动画
ani = FuncAnimation(fig_anim, update, frames=int(t_max * 2), interval=50, blit=True)
plt.show()

# 保存动画（可选）
# ani.save('missile_obscuration.mp4', writer='ffmpeg', fps=20)